#include <ros/ros.h>

#include "core/common/core.h"
#include "perception/sensors/convert/msg_convert.h"
#include "perception/sensors/lidar_2d_undistortion.h"

class TestUndistortion{
    public:
        TestUndistortion();
        ~TestUndistortion();


    private:
        // 二维雷达回调函数
        void LidarCallback(const sensor_msgs::LaserScan::Ptr& msg);
        // IMU回调函数
        void ImuCallback(const sensor_msgs::Imu::Ptr& msg);

    private:
        ros::NodeHandle nh_;

        ros::Subscriber lidarScanSub_;
        ros::Subscriber imuSub_;
        ros::Publisher pointcloud_raw_pub_;
        ros::Publisher pointcloud_undistortion_pub_;
        ros::Publisher scan_undistortion_pub_;

        Lidar2DUndistortion lidar2dUndistortion_;
};


TestUndistortion::TestUndistortion(){
    lidarScanSub_ = nh_.subscribe("/scan_raw", 10, &TestUndistortion::LidarCallback, this);
    imuSub_ = nh_.subscribe("/imu_raw", 10, &TestUndistortion::ImuCallback, this);

    pointcloud_raw_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/pointcloud_raw", 1);
    pointcloud_undistortion_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/pointcloud_undistortion", 1);
    scan_undistortion_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan_undistortion", 1);
}

TestUndistortion::~TestUndistortion(){
    
}

void TestUndistortion::LidarCallback(const sensor_msgs::LaserScan::Ptr& msg){
    // LOG(INFO) << "[LidarCallback] Receive a lidar scan. angle_min=" << msg->angle_min 
    //     << ", angle_max=" << msg->angle_max
    //     << ", angle_increment=" << msg->angle_increment
    //     << ", range_min=" << msg->range_min
    //     << ", range_max=" << msg->range_max
    //     << ", ranges.size=" << msg->ranges.size();

    auto output_msg = lidar2dUndistortion_.getLidarData(msg);
    if(output_msg == nullptr){
        LOG(ERROR) << "[LidarCallback] Failed to get lidar scan data.";
        return;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr =msg_convert::toPointCloudXYZ(msg);   
    sensor_msgs::PointCloud2::Ptr pointcloud_msg_ptr = msg_convert::toPointCloud2(pcl_ptr, msg->header.frame_id);
    pointcloud_raw_pub_.publish(*pointcloud_msg_ptr);

    pointcloud_msg_ptr = msg_convert::toPointCloud2(output_msg, msg->header.frame_id);
    pointcloud_undistortion_pub_.publish(*pointcloud_msg_ptr);

    auto scan_undistortion_ptr = msg_convert::toLaserScan(output_msg);
    if(scan_undistortion_ptr){
        scan_undistortion_ptr->header = msg->header;
        scan_undistortion_pub_.publish(*scan_undistortion_ptr);
    }
}

void TestUndistortion::ImuCallback(const sensor_msgs::Imu::Ptr& msg){
    // LOG(INFO) << "[ImuCallback] Receive a imu msg. angular_velocity.x=" << msg->angular_velocity.x
    //     << ", angular_velocity.y=" << msg->angular_velocity.y
    //     << ", angular_velocity.z=" << msg->angular_velocity.z
    //     << ", linear_acceleration.x=" << msg->linear_acceleration.x
    //     << ", linear_acceleration.y=" << msg->linear_acceleration.y
    //     << ", linear_acceleration.z=" << msg->linear_acceleration.z;
    lidar2dUndistortion_.addImuData(msg);
}

int main(int argc, char** argv){
    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "test_lidar2d_undistortion");

    ros::init(argc, argv, "test_lidar2d_undistortion");
    ros::NodeHandle nh;;
    TestUndistortion testUndistortion;
    ros::spin();

    common::glog::shutdown();
    return 0;
}
